#include "StdAfx.h"
#include "ComPortHandler.h"
using namespace Utility;

ComPortHandler::ComPortHandler(LogFileWriter& logger): m_hCom(NULL), mLogger(logger)
{
	mIsReadingComPort = false;
	mIsComPortOpen = false;
	for (int i=0; i<3; ++i)
	{
		mGyro[i] =0.0;
		mAcc[i] = 0.0;
		mMag[i] = 0.0;
		mQuat[i] =0.0;
	}
	mQuat[3] = 0.0;
	mRoll = 0.0;
	mPitch = 0.0;
	mYaw = 0.0;
}

ComPortHandler::~ComPortHandler(void)
{
}

void ComPortHandler::setWriteCommand(byte command )
{
	mWriteCommand[0] = 0xFA;
	mWriteCommand[1] = mDeviceID;
	mWriteCommand[2] = command;
	mWriteCommand[3] = 0x00;

	byte checkSum = 0;
	for (int i=0; i<4; ++i)
	{
		checkSum ^= mWriteCommand[i];
	}
	mWriteCommand[4] = checkSum;
}

void ComPortHandler::setDeviceID( byte deviceID )
{
	mDeviceID = deviceID;
}

void ComPortHandler::setComPortID( byte comID )
{
	mComPortID = comID;
}

byte ComPortHandler::getComPortID()
{
	return mComPortID;
}

bool ComPortHandler::openComPort()
{
	//create in asynchronous way
	TCHAR tempStr[7]={'\0'};
	wsprintf(tempStr, L"COM%d", mComPortID);
	m_hCom = CreateFile(tempStr,
						GENERIC_READ | GENERIC_WRITE,0,NULL,
						OPEN_EXISTING,
						FILE_ATTRIBUTE_NORMAL|FILE_FLAG_OVERLAPPED, 
						NULL);
	if (m_hCom == INVALID_HANDLE_VALUE) 
	{
		::CloseHandle(m_hCom);
		return false;
	}

	//set i/o buffer
	SetupComm(m_hCom,1024,1024);

	//set time out
	COMMTIMEOUTS CommTimeOuts;
	memset(&CommTimeOuts,0,sizeof (CommTimeOuts));
	CommTimeOuts.ReadIntervalTimeout = MAXWORD;
	CommTimeOuts.ReadTotalTimeoutMultiplier = 0;
	CommTimeOuts.ReadTotalTimeoutConstant = 0;
	
	CommTimeOuts.WriteTotalTimeoutMultiplier = 100;
	CommTimeOuts.WriteTotalTimeoutConstant = 500;
	SetCommTimeouts(m_hCom,&CommTimeOuts);

	//set DCB struct
	DCB		dcb;
	GetCommState(m_hCom,&dcb);
	dcb.BaudRate			= 115200;
	dcb.fBinary				= TRUE;
	dcb.fParity				= FALSE;
	dcb.StopBits			= ONESTOPBIT;
	dcb.ByteSize			= 8;
	dcb.Parity				= NOPARITY;
	dcb.fOutxCtsFlow		= FALSE;
	dcb.fOutxDsrFlow		= FALSE;
	dcb.fDtrControl			= DTR_CONTROL_DISABLE;
	dcb.fDsrSensitivity		= FALSE;
	dcb.fTXContinueOnXoff	= FALSE;
	dcb.fOutX				= FALSE;
	dcb.fInX				= FALSE;
	dcb.fErrorChar			= 0;
	dcb.fNull				= FALSE;
	dcb.fRtsControl			= RTS_CONTROL_DISABLE;
	dcb.fAbortOnError		= TRUE;
	if (!SetCommState(m_hCom, &dcb)) 
	{
		::CloseHandle(m_hCom);
		return false;
	}
	PurgeComm(m_hCom,PURGE_TXCLEAR|PURGE_RXCLEAR);
	mIsComPortOpen = true;
	return true;
}

bool ComPortHandler::writeCommandToComPort()
{
	//use asynchronous write
	OVERLAPPED cmdWriter;
	memset(&cmdWriter,0,sizeof(OVERLAPPED));
	cmdWriter.hEvent=CreateEvent(NULL,TRUE,FALSE,NULL);

	COMSTAT ComStat;
	DWORD dwErrorFlags;
	ClearCommError(m_hCom,&dwErrorFlags,&ComStat);

	BOOL bWriteState;
	DWORD dwBytesWrite=5;
	bWriteState=WriteFile(m_hCom, mWriteCommand, dwBytesWrite, &dwBytesWrite, &cmdWriter);

	if(!bWriteState)
	{
		DWORD waitResult;
		if(GetLastError()==ERROR_IO_PENDING)
		{
			waitResult = WaitForSingleObject(cmdWriter.hEvent,DEF_WRITECMD_TW);
			switch(waitResult)
			{
				case WAIT_OBJECT_0:
					return GetOverlappedResult(m_hCom, &cmdWriter, &dwBytesWrite, FALSE)? true : false;            
				default:
					return false;
			}
		}
		else
			return false;
	}
	return true;
}

void ComPortHandler::readFromComPort()
{
	//64bytes data + 4bytes checkSum
	byte dataContainer[69] = {'\0'};

	byte *pByteData = dataContainer;
	float *pFloatData = reinterpret_cast<float*>(pByteData);
	byte *pCurData;
	byte dataLength, checkSum;  

LoopBlockRx:
	pCurData = pByteData ;
	checkSum = 0;
	int	counter=0;
LoopRx:
	if (!isReadingComPort())
		return;
	bool tryRead = readBytesFromComPort(pCurData, 1);
	if (!tryRead)
	{
// 		TRACE("read failed! Begin Reread!\n");
		mLogger.writeStringToFile("Read failed, begin reread.");
		mLogger.flushAndNewLine();
		goto LoopBlockRx;
	}
	switch (counter)
	{
		case 0 : // FA
			if( pCurData[0] == 0xFA )	
				break;
			else
				goto LoopBlockRx;
		case 1 : // FF
			if( pCurData[0] == mDeviceID )
				break;
			else
				goto LoopBlockRx;
		case 2 : // MID
			if( (pCurData[0] & 0x80) == 0x80 ) 
				break;
			else
				goto LoopBlockRx;	
		case 3 : //LenL
			dataLength = pCurData[0];						
			break;
		default:
			if(counter != (dataLength + 4)) 
				break;
			else
			{
				for(int i = 0; i < (dataLength + 4); i++) 
					checkSum ^= pByteData[i];
				if(checkSum != pByteData[dataLength + 4]) 
					goto LoopBlockRx;
				if(pByteData [2]== 0x80)
				{
					handleReadData(pFloatData);
				}
				goto LoopBlockRx;
			}//end else
	}//switch break
	counter++;
	pCurData++;	

	goto LoopRx;	
}

void ComPortHandler::startReadFromComPort()
{
	mIsReadingComPort = true;
	AfxBeginThread(ReadThreadFuc, this);
}

void ComPortHandler::endReadFromComPort()
{
	mIsReadingComPort = false;
}

bool ComPortHandler::isReadingComPort()
{
	return mIsReadingComPort;
}

bool ComPortHandler::readBytesFromComPort( void* pReadData, int numOfBytes /*= 1*/ )
{
	OVERLAPPED dataReader;
	memset(&dataReader,0,sizeof(OVERLAPPED));
	dataReader.hEvent=CreateEvent(NULL,TRUE,FALSE,NULL);

	DWORD dwBytesRead=numOfBytes; 

	COMSTAT ComStat;
	DWORD dwErrorFlags;
	ClearCommError(m_hCom,&dwErrorFlags,&ComStat);
// 	TRACE("bytes(not read) in comport: %d\n", ComStat.cbInQue);
	if ( dwErrorFlags != 0 )
	{
// 		TRACE("error code: %x\n", dwErrorFlags);
		mLogger.writeStringToFile("error code: " + Helper::getHexValueString(dwErrorFlags));
		mLogger.flushAndNewLine();
		PurgeComm(m_hCom, PURGE_TXABORT|PURGE_RXABORT|PURGE_TXCLEAR|PURGE_RXCLEAR);
	}
	BOOL bReadState=ReadFile(m_hCom, pReadData, dwBytesRead, &dwBytesRead, &dataReader);
	if(!bReadState)
	{
		if(GetLastError() == ERROR_IO_PENDING)
		{
			DWORD waitResult = WaitForSingleObject(dataReader.hEvent,DEF_READ_TW);
			switch(waitResult)
			{
				case WAIT_OBJECT_0:
					return GetOverlappedResult(m_hCom, &dataReader, &dwBytesRead, FALSE)? true : false;            
				default:
					return false;
			}
		}
		return false;
	}
	return true;
}

void ComPortHandler::handleReadData( float* pData )
{
	mGyro[0] = pData[1];
	mGyro[1] = pData[2];
	mGyro[2] = pData[3];

	mAcc[0]  = pData[4];
	mAcc[1]  = pData[5];
	mAcc[2]  = pData[6];

	mMag[0]  = pData[7];
	mMag[1]  = pData[8];
	mMag[2]  = pData[9];

	mQuat[0] = pData[10];
	mQuat[1] = pData[11];
	mQuat[2] = pData[12];
	mQuat[3] = pData[13];

	mRoll    = pData[14];
	mPitch   = pData[15];
	mYaw     = pData[16];
}

float* ComPortHandler::getGyro()
{
	return mGyro;
}

float* ComPortHandler::getAcc()
{
	return mAcc;
}

float* ComPortHandler::getMag()
{
	return mMag;
}

float* ComPortHandler::getQuat()
{
	return mQuat;
}

float ComPortHandler::getRoll()
{
	return mRoll;
}

float ComPortHandler::getPitch()
{
	return mPitch;
}

float ComPortHandler::getYaw()
{
	return mYaw;
}

void ComPortHandler::getRotateValueByQuat( float& rAngle, float& rX, float& rY, float& rZ )
{
	float w = mQuat[0];
	float x = mQuat[1];
	float y = mQuat[2];
	float z = mQuat[3];
	float scale = sqrt(x*x + y*y + z*z);
	if (scale < (1e-9))
	{
		return;
	}
	rX = x / scale;
	rY = y / scale;
	rZ = z / scale;
	rAngle = acos(w) * 2.0;
	rAngle = rAngle *180/3.141592654;
}

void ComPortHandler::closeComPortHanle()
{
	CloseHandle(m_hCom);
	mIsComPortOpen = false;
	m_hCom = NULL;
}

bool ComPortHandler::isComPortOpen()
{
	return mIsComPortOpen;
}

UINT ReadThreadFuc( LPVOID pParam )
{
	ComPortHandler* pHandler = (ComPortHandler*) pParam;
	pHandler->readFromComPort();
	return 0;
}
